#include <iostream>
#include <fstream>
#include "stdio.h"
#include "string.h"

using namespace std;

#define DATA_FILE "log.txt"
#define RESULT_FILE "result.txt"

typedef unsigned short uint16_t;
typedef short int16_t;

uint16_t convertToTempDelta(float pidValue)
{
	///??????
	int16_t tmpData = (int16_t)pidValue;
    const float baseMax = 12650.0;

    float tempData_f = pidValue * 100 / baseMax;

    tempData_f = 100 - tempData_f;

	if(tempData_f<0)
	{
		tmpData = 0;
	}
	else if(tempData_f > 100)
    {
    	tmpData = 100;
    }
	else
	{
		tmpData = (int16_t)tempData_f;
	}

    return tmpData;
}

float const temp2res[] =
{
	96.09,  96.48,  96.87,  97.26,  97.65,  98.04,  98.44,  98.83,  99.22,  99.61,	// -10 ~ -1
	100.00, 100.39, 100.78, 101.17, 101.56, 101.95, 102.34, 102.73, 103.12, 103.51,	// 0~9
	103.90, 104.29, 104.68, 105.07, 105.46, 105.85, 106.24, 106.63, 107.02, 107.40,	// 10~19
	107.79, 108.18, 108.57, 108.96, 109.35, 109.73, 110.12, 110.51, 110.90, 111.29,	// 20~29
	111.67, 112.06, 112.45, 112.83, 113.22, 113.61, 114.00, 114.38, 114.77, 115.15,	// 30~39
	115.54, 115.93, 116.31, 116.70, 117.08, 117.47, 117.86, 118.24, 118.63, 119.01,	// 40~49
	119.40, 119.78, 120.17, 120.55, 120.94, 121.32, 121.71, 122.09, 122.47, 122.86,	// 50~59
	123.24, 123.63, 124.01, 124.39, 124.78, 125.16, 125.54, 125.93, 126.31, 126.69,	// 60~69
	127.08, 127.46, 127.84, 128.22, 128.61, 128.99, 129.37, 129.75, 130.13, 130.52,	// 70~79
	130.90, 131.28, 131.66, 132.04, 132.42, 132.80, 133.18, 133.57, 133.95, 134.33,	// 80~89
	134.71, 135.09, 135.47, 135.85, 136.23, 136.61, 136.99, 137.37, 137.75, 138.13,	// 90~99
	138.51, 138.88, 139.26, 139.64, 140.02, 140.40, 140.78, 141.16, 141.54, 145.69, // 100~109
	142.29,	142.67,	143.05,	143.43,	143.80,	144.18,	144.56,	144.94,	145.31,	145.69,	// 110~119
	146.07,	146.44,	146.82,	147.20,	147.57, 147.95, 148.33, 148.70, 149.08, 149.49,	// 120~129
	149.83, 150.21, 150.58, 150.96, 151.33, 151.71, 152.08, 152.46, 152.83, 153.21, // 130~139
	153.58, 153.96, 154,33, 154.71, 155.08, 155.46, 155.83, 156.20, 156.58, 156.95, // 140~149
	157.33, 157.70, 158.07, 158.45, 158.82, 159.19, 159.56, 159.94, 160.31, 160.68, // 150~159
	161.05, 161.43, 161.80, 162.17, 162.54, 162.91, 163.29, 163.66, 164.03, 164.40, // 160~169
	164.77, 165.14, 165.51, 165.89, 166.26, 166.63, 167.00, 167.37, 167.74, 168.11,	// 170~179
	168.48, 168.85, 169.22, 169.59, 169.96, 170.33, 170.70, 171.07, 171.43, 171.80,	// 180~189
	172.17, 172.54, 172.91, 173.28, 173.65, 174.02, 174.38, 174.75, 175.12, 175.49	// 190~199
};

#define Au	10

unsigned short convertTempToVoltage(float tempIn)
{
	float resRead = 0;
	float delta = 0;
	float resCalc = 0;

	unsigned short retValue = 0;

    float valtageCalc = 0;
	int i = (int)(tempIn+10);

    printf("index is %d\n",i);

	if(tempIn<-10 || tempIn>199)
    {
        return 0;
    }

	resRead = temp2res[i];

	delta = tempIn - i + 10;
	resCalc = resRead + delta*0.385;

    printf("resRead:%f,delta:%f,rescalc:%f\n",resRead,delta,resCalc);

	valtageCalc = resCalc*0.00124*Au*65535.00/4.09600;

	retValue = (unsigned short)valtageCalc;

    return retValue;
}


int main(int argc, char * argv[])
{
    ifstream fin(DATA_FILE);
    ofstream fout(RESULT_FILE);
    char buff[1024];

    char * ptrTemp = NULL;

    float tempData = 0;
    int tempData1 = 0;

    float lastTemp = 0;
    int lastTemp1 = 0;

    float pidValue = 0;

    float pidValue1 = 0;
    float tempDelta = 0;
    int tempDelta1 = 0;

    float err[2] = {0,0};
    float errLast[2] = {0,0};

    float integral[2] = {0,0};


    const float kd = 0.003;
    const float ki = 1.675;
    const float kp = 3.285;//35.72034;

    uint16_t pwm_dutyCycle = 0; 

    int targetValue = convertTempToVoltage(70);

    int threshold = convertTempToVoltage(59);

    while(!fin.eof())
    {
        memset(buff,0,1024);
        fin.getline(buff,1024);
        ptrTemp = strstr(buff,"temperature");
        if(ptrTemp)
        {
           ptrTemp+=11;

           sscanf(ptrTemp,"[%f]",&tempData);

           if(tempData>50)
           {
                tempDelta = tempData - lastTemp;
                if(tempDelta < 3 && tempDelta > -3)
                {
                    err[0] =  70.0 - tempData;
                    integral[0] += err[0];
                    pidValue = kp*err[0] + ki*integral[0] + kd*(err[0]-errLast[0]);
                    pwm_dutyCycle = convertToTempDelta(pidValue);
                }
                else
                {
                    pidValue = 0;
                    integral[0] = 0;
                }
                errLast[0] = err[0];
                lastTemp = tempData;
           }
           else
           {
               pidValue = 0;
               pwm_dutyCycle = 0;
           }

           fout<<tempData<<'\t'<<pidValue<<'\t'<<pwm_dutyCycle<<'\t';
           
        }
        ptrTemp = strstr(buff,"quantity");
        if(ptrTemp)
        {
           ptrTemp+=8;

           sscanf(ptrTemp,"[%d]",&tempData1);

           if(tempData1>threshold)
           {
                tempDelta1 = tempData1 - lastTemp1;
                if(tempDelta1 < 231 && tempDelta1 > -231)
                {
                    err[1] =  targetValue - tempData1;
                    integral[1] += err[1];
                    pidValue1 = kp*err[1] + ki*integral[1] + kd*(err[1]-errLast[1]);
                    pwm_dutyCycle = convertToTempDelta(pidValue1);
                }
                else
                {
                    pidValue1 = 0;
                    integral[1] = 0;
                }
                errLast[1] = err[1];
                lastTemp1 = tempData1;
           }
           else
           {
               pidValue1 = 0;
               pwm_dutyCycle = 0;
           }

           fout<<tempData1<<'\t'<<pidValue1<<'\t'<<pwm_dutyCycle<<"\r\n";
           
        }
    } 

    fin.close();
    fout.close();

}

